//
// Created by 74354 on 2022/1/14.
//

#pragma once

namespace myFilter
{
    class LowPassFilter2p
    {
    public:
        // constructor
        LowPassFilter2p(float sample_freq, float cutoff_freq) {
            // set initial parameters
            set_cutoff_frequency(sample_freq, cutoff_freq);
            _delay_element_1 = _delay_element_2 = 0;
        }

        // change parameters
        void set_cutoff_frequency(float sample_freq, float cutoff_freq);

        // apply - Add a new raw value to the filter
        // and retrieve the filtered result
        float apply(float sample);

        // return the cutoff frequency
        float get_cutoff_freq(void) const {
            return _cutoff_freq;
        }

    private:
        float           _cutoff_freq;
        float           _a1;
        float           _a2;
        float           _b0;
        float           _b1;
        float           _b2;
        float           _delay_element_1;        // buffered sample -1
        float           _delay_element_2;        // buffered sample -2
    };


    /*
     * Predict:
     * X(t)_ = F*X(t-1) + B*U(t-1) + Wk;  先验估计
     * P(t)_ = F*P(t-1)*F.T + Q;     计算先验估计的协方差矩阵
     *
     * Update:
     * K(t) = P(t)_*H.T*inv(H*P(t)_*H.T + R);   更新卡尔曼增益
     * X(t) = X(t)_ + B*U(t) + K(t)*(Z(t) - H*X(t)_)    利用先验估计值计算后验估计值
     * P(t) = (I - K*H)*P(t)_       更新先验估计的协方差矩阵
     */

    /* 一阶卡尔曼滤波 */
    template<typename T>
    class Kalman1P{
    public:
        Kalman1P(){}
        Kalman1P(T f,T q,T r,T p):F(f),Q(q),R(r),Pt_1(p){}
        inline T addValue(T value);

    private:
        float Xt_1;    // 上一次值
        float Kt_1;   // 卡尔曼增益
        float F;            // 状态转移矩阵(常量)
        float Q;            // 过程噪声的方差(常量)
        float R;            // 观测噪声的方差(常量)
        float Pt_1;         // 先验估计协方差矩阵(每次更新后改变)
    };
    template<typename T>
    inline T Kalman1P<T>::addValue(T value) {
        T Xt_ = F*Xt_1;         // 先验估计
        T Pt_ = F*Pt_1*F + Q;
        T Kt = Pt_ / (Pt_ + R);
        T Xt = Xt_ + Kt*(value - Xt_);
        T Pt = (1 - Kt)*Pt_;
        Pt_1 = Pt;
        Xt_1 = Xt;
        Kt_1 = Kt;
        return Xt;
    }


} // namespace math


